import network, time, socket, gc
from machine import UART
from maix import GPIO
from fpioa_manager import fm
import utime
import sensor, image, lcd
import ustruct as struct

# MQTT协议相关函数
class MQTTException(Exception):
    pass

class MQTTClient:

    def __init__(self, client_id, server, port=0, user=None, password=None, keepalive=0,
                 ssl=False, ssl_params={}):
        if port == 0:
            port = 8883 if ssl else 1883
        self.client_id = client_id
        self.sock = None
        self.server = server
        self.port = port
        self.ssl = ssl
        self.ssl_params = ssl_params
        self.pid = 0
        self.cb = None
        self.user = user
        self.pswd = password
        self.keepalive = keepalive
        self.lw_topic = None
        self.lw_msg = None
        self.lw_qos = 0
        self.lw_retain = False

    def _send_str(self, s):
        self.sock.write(struct.pack("!H", len(s)))
        self.sock.write(s)

    def _recv_len(self):
        n = 0
        sh = 0
        while 1:
            b = self.sock.read(1)[0]
            n |= (b & 0x7f) << sh
            if not b & 0x80:
                return n
            sh += 7

    def set_callback(self, f):
        self.cb = f

    def set_last_will(self, topic, msg, retain=False, qos=0):
        assert 0 <= qos <= 2
        assert topic
        self.lw_topic = topic
        self.lw_msg = msg
        self.lw_qos = qos
        self.lw_retain = retain

    def connect(self, clean_session=True):
        while True:
            gc.collect()  # 清理内存垃圾，延缓出现不够用的报错
            addr = socket.getaddrinfo(self.server, self.port)[0][-1]  # 获取服务器ip和端口
            print(addr)
            sock = socket.socket()  # 使用给定的地址系列,套接字类型和协议号创建一个新套接字
            try:
                sock.connect(addr)  #尝试连接服务器
                print("连接成功")
                self.sock = sock
                break
            except Exception as e: #返回错误信息
                print("连接失败:", e)
                sock.close()
            sock.settimeout(5)

        if self.ssl:
            import ussl
            self.sock = ussl.wrap_socket(self.sock, **self.ssl_params)
        premsg = bytearray(b"\x10\0\0\0\0\0")
        msg = bytearray(b"\x04MQTT\x04\x02\0\0")

        sz = 10 + 2 + len(self.client_id)
        msg[6] = clean_session << 1
        if self.user is not None:
            sz += 2 + len(self.user) + 2 + len(self.pswd)
            msg[6] |= 0xC0
        if self.keepalive:
            assert self.keepalive < 65536
            msg[7] |= self.keepalive >> 8
            msg[8] |= self.keepalive & 0x00FF
        if self.lw_topic:
            sz += 2 + len(self.lw_topic) + 2 + len(self.lw_msg)
            msg[6] |= 0x4 | (self.lw_qos & 0x1) << 3 | (self.lw_qos & 0x2) << 3
            msg[6] |= self.lw_retain << 5

        i = 1
        while sz > 0x7f:
            premsg[i] = (sz & 0x7f) | 0x80
            sz >>= 7
            i += 1
        premsg[i] = sz

        self.sock.write(premsg, i + 2)
        self.sock.write(msg)
        self._send_str(self.client_id)
        if self.lw_topic:
            self._send_str(self.lw_topic)
            self._send_str(self.lw_msg)
        if self.user is not None:
            self._send_str(self.user)
            self._send_str(self.pswd)

        resp = []

        while not resp:# 一直读到服务器有应答为止
            try:
                resp = self.sock.read(4)
                break
            except Exception as e: #返回错误信息
                print("read fail:", e)
        assert resp[0] == 0x20 and resp[1] == 0x02
        if resp[3] != 0:
            raise MQTTException(resp[3])
        return resp[2] & 1

    def disconnect(self):
        self.sock.write(b"\xe0\0")
        self.sock.close()

    def ping(self):
        self.sock.write(b"\xc0\0")

    def publish(self, topic, msg, retain=False, qos=0):
        pkt = bytearray(b"\x30\0\0\0")
        pkt[0] |= qos << 1 | retain
        sz = 2 + len(topic) + len(msg)
        if qos > 0:
            sz += 2
        assert sz < 2097152
        i = 1
        while sz > 0x7f:
            pkt[i] = (sz & 0x7f) | 0x80
            sz >>= 7
            i += 1
        pkt[i] = sz
        #print(hex(len(pkt)), hexlify(pkt, ":"))
        self.sock.write(pkt, i + 1)
        self._send_str(topic)
        if qos > 0:
            self.pid += 1
            pid = self.pid
            struct.pack_into("!H", pkt, 0, pid)
            self.sock.write(pkt, 2)
        self.sock.write(msg)
        if qos == 1:
            while 1:
                op = self.wait_msg()
                if op == 0x40:
                    sz = self.sock.read(1)
                    assert sz == b"\x02"
                    rcv_pid = self.sock.read(2)
                    rcv_pid = rcv_pid[0] << 8 | rcv_pid[1]
                    if pid == rcv_pid:
                        return
        elif qos == 2:
            assert 0

    def subscribe(self, topic, qos=0):
        assert self.cb is not None, "Subscribe callback is not set"
        pkt = bytearray(b"\x82\0\0\0")
        self.pid += 1
        struct.pack_into("!BH", pkt, 1, 2 + 2 + len(topic) + 1, self.pid)
        #print(hex(len(pkt)), hexlify(pkt, ":"))
        self.sock.write(pkt)
        self._send_str(topic)
        self.sock.write(qos.to_bytes(1, "little"))
        while 1:
            op = self.wait_msg()
            if op == 0x90:
                resp = self.sock.read(4)
                #print(resp)
                assert resp[1] == pkt[2] and resp[2] == pkt[3]
                if resp[3] == 0x80:
                    raise MQTTException(resp[3])
                return

    # Wait for a single incoming MQTT message and process it.
    # Subscribed messages are delivered to a callback previously
    # set by .set_callback() method. Other (internal) MQTT
    # messages processed internally.
    def wait_msg(self):
        res = self.sock.read(1)
        self.sock.setblocking(True)
        if res is None:
            return None
        if res == b"":
            #raise OSError(-1)
            return None
        if res == b"\xd0":  # PINGRESP
            sz = self.sock.read(1)[0]
            assert sz == 0
            return None
        op = res[0]
        if op & 0xf0 != 0x30:
            return op
        sz = self._recv_len()
        topic_len = self.sock.read(2)
        topic_len = (topic_len[0] << 8) | topic_len[1]
        topic = self.sock.read(topic_len)
        sz -= topic_len + 2
        if op & 6:
            pid = self.sock.read(2)
            pid = pid[0] << 8 | pid[1]
            sz -= 2
        msg = self.sock.read(sz)
        self.cb(topic, msg)
        if op & 6 == 2:
            pkt = bytearray(b"\x40\x02\0\0")
            struct.pack_into("!H", pkt, 2, pid)
            self.sock.write(pkt)
        elif op & 6 == 4:
            assert 0

    # Checks whether a pending message from server is available.
    # If not, returns immediately with None. Otherwise, does
    # the same processing as wait_msg.
    def check_msg(self):
        self.sock.setblocking(False)
        return self.wait_msg()




###### WiFi模块初始化 ######
#串口初始化
fm.register(13, fm.fpioa.UART2_RX, force=True)
fm.register(14, fm.fpioa.UART2_TX, force=True)
uart = UART(UART.UART2,115200,timeout=1000,read_buf_len=4096)

def wifi_init():
    global uart
    time.sleep_ms(200)
    time.sleep(2)
    uart = UART(UART.UART2,115200,timeout=1000, read_buf_len=4096)
    tmp = uart.read()
    uart.write("AT+UART_CUR=921600,8,1,0,0\r\n")
    print(uart.read())
    uart = UART(UART.UART2,921600,timeout=1000, read_buf_len=(10240)) # important! baudrate too low or read_buf_len too small will loose data
    uart.write("AT\r\n")
    tmp = uart.read()
    print(tmp)
    if not tmp.endswith("OK\r\n"):
        print("reset fail")
        return None
    try:
        nic = network.ESP8285(uart)
    except Exception:
        return None
    return nic

def wifi_reset():
    global uart
    time.sleep_ms(200) #延时200 ms
    time.sleep(2)  #延时2 s
    uart = UART(UART.UART2,115200,timeout=1000, read_buf_len=4096)
    #uart:UART 号，使用指定的 UART
    #baudrate:UART 波特率
    #timeout: 串口接收超时时间
    #read_buf_len： 串口接收缓冲，串口通过中断来接收数据，如果缓冲满了，将自动停止数据接收
    #读取串口缓冲中的数据到 tmp 中
    uart.write("AT+UART_CUR=921600,8,1,0,0\r\n")
    #发送数据
    print(uart.read())#打印串口读取的数据
    uart = UART(UART.UART2,921600,timeout=1000, read_buf_len=4096)
    uart.write("AT\r\n")
    tmp = uart.read()
    print(tmp)

    if not (tmp == b"\r\nOK\r\n" or tmp == b'AT\r\r\n\r\nOK\r\n'):   #检查tmp是否以 OK\r\n 结尾，是的话就返回True
        print("reset fail")
        return None
    try:
        nic = network.ESP8285(uart)  #尝试初始化ESP8285，如果失败就跳到下列的 except 语句中
    except Exception:
        print("esp8285初始化失败，", end="")
        return None
    return nic

############################
#########  主程序  ##########
############################
nic = None  #清空 nic 内值

while True:
    if not nic or not nic.isconnected():
        nic = wifi_reset()  #如果wifi没有连接就重新使能，如果使能成功会将nic赋值True
    if not nic:  #使能失败
        print("wifi 重置失败")
        continue  #进行下一轮使能
    try:
        print("尝试连接wifi")
        nic.connect("test", "1145141919810")  #尝试连接wifi
        ifconfig = nic.ifconfig() #查看wifi连接信息
        print("wifi连接成功！当前网络信息如下：\nIP地址"+ifconfig[0]+"\n子网掩码"+ifconfig[1]+"\n网关"+ifconfig[2]+"\nDNS服务器"+ifconfig[3]+"\n")
        break
    except Exception:
        print("WiFi 连接失败")
        continue  #连接失败进行下一轮连接
    if not nic.isconnected():
        print("WiFi 连接失败")
        continue

err = 0

lcd.init()
sensor.reset(dual_buff=False)
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 2000)

SERVER = 'broker.emqx.io'
PORT = 1883
CLIENT_ID = '01Studio-K210' # 客户端ID
TOPIC = '/public/01Studio/1' # TOPIC名称
client = MQTTClient(CLIENT_ID, SERVER, PORT)
print("a")
client.connect()

while True:
    img = sensor.snapshot()
    lcd.display(img)
    img = img.compress(quality=60)
    img_bytes = img.to_bytes()
    block = int(len(img_bytes)/2048)
    for i in range(block):
        send_len = client.publish(TOPIC, img_bytes[i*2048:(i+1)*2048])
    send_len2 = client.publish(TOPIC, img_bytes[block*2048:])
    print(block)
    print(client.wait_msg())
    utime.sleep(1)
